/*!
  @file triangulation.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _TRIANGULATION_H_
#define _TRIANGULATION_H_

#include <boost/scoped_ptr.hpp>

#include "mrover_vo/camera_params.h"
#include "mrover_vo/feature.h"

namespace vo {

/*! 
 *  @brief  Stereo triangulation
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

class Triangulation {
  typedef CameraParams::ConstPtr CameraParamsConstPtr;
  typedef typename Feature::ConstPtr FeatureConstPtr;
  typedef boost::shared_ptr<Feature::ScenePoints> ScenePointsPtr;

 public:
  typedef boost::scoped_ptr<Triangulation> Ptr;

  Triangulation() { }

  ~Triangulation() { }


  inline void compute(ScenePointsPtr &sp) {
    setOutputScenePoint(sp);
    compute();
  }
  void compute();

  void setInputFeature(const FeatureConstPtr &feature) { feature_ = feature; }
  void setOutputScenePoint(const ScenePointsPtr &sp) { sp_ = sp; }

  void setCameraParams(const CameraParamsConstPtr &cam_params) { 
    P = cam_params->P;
  }

 private:
  bool initCompute() {
    if (!feature_ || !sp_ || !P[0].data || !P[1].data) return false;
    return true;
  }

  //! Projection matrices for left(0) and right(1) cameras
  std::vector<cv::Mat> P;

  FeatureConstPtr feature_;
  ScenePointsPtr sp_;
};

}  // namespace myns

#endif  // _TRIANGULATION_H_
